import utm
import numpy as np


# 正变换 wgs84转横墨卡托
def get_utm(lon, lat):
    [east, north, zone_number, zone_letter] = utm.from_latlon(latitude=lat, longitude=lon)
    # print('east is %0.3f m,north is %0.3f m' % (east, north))
    return [east, north, zone_number, zone_letter]


# 逆变换 横墨卡托转WGS84
def get_wgs84(east, north, zone_number, zone_letter):
    lat, lon = utm.to_latlon(east, north, zone_number, zone_letter)
    # print('lon is %0.8f, lat is %0.8f' % (lon, lat))
    return [lon, lat]


def get_list_wgs84(init_gps84, points_xyz):
    '''
    以初始经纬度为坐标原点 转换所有局部大地坐标系下的点坐标序列
    :param init_gps84: 初始经纬度 [lon lat]
    :param points_xyz: 局部坐标点序列 n*[北 东 地 横滚角 俯仰角 艏向角] list[ndarray]
    :return: 经纬度格式点序列
    '''
    points_wgs84 = np.zeros(shape=[len(points_xyz), 2])
    local = get_utm(init_gps84[0], init_gps84[1])
    for var in range(len(points_xyz)):
        points_wgs84[var, :] = get_wgs84(east=(local[0] + points_xyz[var][1]),
                                         north=(local[1] + points_xyz[var][0]),
                                         zone_number=local[2],
                                         zone_letter=local[3])

    return points_wgs84

# lon = 120.30754612708418
# lat = 31.49450233916176
# east, north = get_utm(lon, lat, False)
# ln, lt = get_wgs84(east, north, True)


# lon1 = 120.255310058594
# lat1 = 31.490659713745
# lon2 = 120.25586691049563
# lat2 = 31.491547123109701
# local1 = get_utm(lon1, lat1)
# local2 = get_utm(lon2, lat2)
# print(local2[0] - local1[0], ',', local2[1] - local1[1])
